#include "pspmath.h"

float vfpu_atanf(float x) {
    float result;
    // result = asinf(x/sqrt(x*x+1))
    __asm__ volatile (
        "mtv      %1, S000\n"
        "vmul.s   S001, S000, S000\n"
        "vadd.s   S001, S001, S001[1]\n"
        "vrsq.s   S001, S001\n"
        "vmul.s   S000, S000, S001\n"
        "vasin.s  S000, S000\n"
        "vcst.s   S001, VFPU_PI_2\n"
        "vmul.s   S000, S000, S001\n"
        "mfv      %0, S000\n"
    : "=r"(result) : "r"(x));
    return result;
}


#define PI   3.14159265358979f
#define PI_2 1.57079632679489f

inline static float fabsf(float x) {
    float r;
    __asm__ volatile( "abs.s %0, %1" : "=f"(r) :"f"(x):"memory");
    return r;
}

/*    double a;

    if (fabs(x) >= fabs(y)) {
        a = atan(y/x) ;
        if (x < 0.0) {
            if (y >= 0.0) a += _pi ;
            else a -= _pi ;
        }
    }
    else {
        a = -atan(x/y) ;
        if (y < 0.0) a -= _halfpi ;
        else     a += _halfpi ;
    }
    return a ;*/

float vfpu_atan2f(float y, float x) {
    float r;

    if (fabsf(x) >= fabsf(y)) {
        r = vfpu_atanf(y/x);
        if   (x < 0.0f) r += (y>=0.0f ? PI : -PI);
    } else {
        r = -vfpu_atanf(x/y);
        r += (y < 0.0f ? -PI_2 : PI_2);
    }
    return r;
}
